/*
 * Center then drive to the end of the hall, 
 * turn around and return to the origional position.
 *
 * Tests driving capabilites
 */

#include <libplayerc++/playerc++.h>
#include <iostream>

#include "args.h"

#define RAYS 32

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;
    
    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);
    //SonarProxy sp(&robot, gIndex);
    std::cout << robot << std::endl;

    pp.SetMotorEnable (true);

    
    double newspeed = 0;
    double newturnrate = 0;
    double minR = 5.6;
    double minL = 5.6;
    int lloc = 0;
    int rloc = 0;
    //int temp;

    robot.Read(); //here so GetRange doesn't segfault
      
    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();

      /* Simple accessor functions 
      std::cout << "Max Range of laser " << lp.GetMaxRange() << std::endl;
      std::cout << "Number of points in scan " << lp.GetCount() << std::endl;
      std::cout << "Bearing " << lp.GetBearing(135) << std::endl;
      getchar();
      */

      /* GetRange() 
      std::cout << "Testing GetRange \n"; double range;
      //range = lp.GetRange(654); // far bottom left
      //range = lp.GetRange(0); // far bottom right
      range = lp.GetRange(327); // directly in front
      std::cout << "Range: " << range << std::endl;
      getchar();
      */

      /* Sonar
      std::cout << "Testing Sonar \n";
      double minR = sp.GetScan(0);
      double minR1 = sp.GetScan(1);
      double minR2 = sp.GetScan(2);
      double minR3 = sp.GetScan(3);
      double minR4 = sp.GetScan(4);
      double minR5 = sp.GetScan(5);
      double minR6 = sp.GetScan(6);
      double minR7 = sp.GetScan(7);

      std::cout << "Sonar(0): " << minR << std::endl;
      std::cout	<< "Sonar(1): " << minR1 << std::endl;
      std::cout	<< "Sonar(2): " << minR2 << std::endl;
      std::cout	<< "Sonar(3): " << minR3 << std::endl;
      std::cout << "Sonar(4): " << minR4 << std::endl;
      std::cout << "Sonar(5): " << minR5 << std::endl;
      std::cout << "Sonar(6): " << minR6 << std::endl;
      std::cout << "Sonar(7): " << minR7 << std::endl;
      std::cout << std::endl;
      */

      /* Get minimum range reading on right and left sides 
      uint count = lp.GetCount();
      for (uint j = 0; j < count/2; ++j){  // find the minimum on the right side
       if (minR > lp[j]){
       minR = lp[j];
       rloc=j;
      }
      }

      for (uint j = count/2; j < count; ++j){ // find the minimum on the left side
       if (minL > lp[j]){
         minL = lp[j];
         lloc=j;
       }
      }
      std::cout << "minR: " << minR << " at index " << rloc << std::endl;
      std::cout << "minL: " << minL << " at index " << lloc << std::endl;

      
      if(minR <= 0.5){
	std::cout << "Oh-shit!\n";
	newspeed = 0.0;
	newturnrate = 0.375;
      }
      else if(minL <= 0.5){
	std::cout << "Oh-shit!\n";
	newspeed = 0.0;
	newturnrate = -0.375;
      }
      else if(minR <= 1.0){
	std::cout << "Uh-oh!\n";
	newspeed = 0.2;
	newturnrate = 0.375;
      }
     
      else if(minL <= 1.0){
	std::cout << "Uh-oh!\n";
	newspeed = 0.2;
	newturnrate = -0.375;
      }
      else{
	newspeed = 0.5;
	newturnrate = 0;
      }
      */

      /* Using only the certain sensors */
      double front = lp[327];
      double right = lp[218];
      double left = lp[437];
      bool turning = false;

      if(right <= 0.5){
	newspeed = 0.0;
	newturnrate = 0.375;
      }
      else if(left <= 0.5){
	newspeed = 0.0;
	newturnrate = -0.375;
      }
      else if(right <= 1.0){
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      else if(left <= 1.0){
	newspeed = 0.2;
	newturnrate = -0.375;
      }
      else if(front <= 1.5){
	newspeed = 0.2;
	newturnrate = 0.375;
      }
      else{
	newspeed = 0.5;
	newturnrate = 0.0;
      }
      
      //printf("Front: %f\nRight: %f\nLeft: %f\n\n", front, right, left);
      pp.SetSpeed(newspeed, newturnrate);
      //getchar();
      
    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
